#include "mpu6050.h"
#define MPU6050_SLAVE_ADDRESS 0xD0
//初始化MPU6050
#define SMPLRT_DIV              0x19 //陀螺仪采样率典型值为0x07 1000/(1+7)=125HZ
#define CONFIG                  0x1A //低通滤波器 典型值0x06 5hz
#define GYRO_CONFIG             0x1B //陀螺仪测量范围 0x18正负2000度
#define ACCEL_CONFIG            0x1C //加速度计测量范围 0x18正负16g
#define ACCEL_CONFIG2           0x1D //加速度计低通滤波器 0x06 5hz

#define USER_CTRL               0x6A //用户配置当为0x10时使用SPI模式
#define PWR_MGMT_1              0x6B //电源管理1典型值为0x00
#define PWR_MGMT_2              0x6C //电源管理2典型值为0x00

#define WHO_AM_I                0x75 //器件ID MPU6050默认返回值为0x68

#define I2C_ADDR_MPU6050        0xD0 //左移一位后的地址           
                              //0x68 //没有左移一位的7位地址

#define ACCEL_XOUT_H            0x3B  //加速度计输出数据
#define ACCEL_XOUT_L            0x3C
#define ACCEL_YOUT_H            0x3D
#define ACCEL_YOUT_L            0x3E
#define ACCEL_ZOUT_H            0x3F
#define ACCEL_ZOUT_L            0x40

#define TEMP_OUT_H              0x41  //温度计输出数据
#define TEMP_OUT_L              0x42

#define GYRO_XOUT_H             0x43  //陀螺仪输出数据
#define GYRO_XOUT_L             0x44
#define GYRO_YOUT_H             0x45
#define GYRO_YOUT_L             0x46
#define GYRO_ZOUT_H             0x47
#define GYRO_ZOUT_L             0x48     
void MPU_Init(void)
{                             /*
                                  MPU_Write_Byte(MPU_PWR_MGMT1_REG,0x80);//解除休眠状态
                                  MPU_Write_Byte(MPU_PWR_MGMT1_REG,0x00);
                                  MPU_Write_Byte(MPU_SAMPLE_RATE_REG,0x07);//陀螺仪采样率，典型值：0x07(125Hz)
                                  MPU_Write_Byte(MPU_CFG_REG,0x06);//低通滤波频率，典型值：0x06(5Hz)
                                  MPU_Write_Byte(MPU_GYRO_CFG_REG,0x18);//陀螺仪自检及测量范围，典型值：0x18(不自检，2000deg/s)
                                  MPU_Write_Byte(MPU_ACCEL_CFG_REG,0x01);//加速计自检、测量范围及高通滤波频率，典型值：0x01(不自检，2G，5Hz)
                                  //ReadByteIIC(MPU_DEVICE_ID_REG);
                                  MPU_Write_Byte(MPU_PWR_MGMT2_REG,0x00);*/
    /*mDelaymS(500);
    MPU_Write_Byte(MPU_PWR_MGMT1_REG, 0x00);   //解除休眠状态
    mDelaymS(500);
    MPU_Write_Byte(MPU_SAMPLE_RATE_REG, 0x07); //陀螺仪采样率
    MPU_Write_Byte(0x1A, 0x06);
    MPU_Write_Byte(0x1C, 0x01);             //配置加速度传感器工作在2G模式
    MPU_Write_Byte(MPU_GYRO_CFG_REG, 0x18); //陀螺仪自检及测量范围，典型值：0x18(不自检，2000deg/s)*/
   /* WriteByteMPU(USER_CTRL, 0x00);       //配置为0x10时为SPI通信模式
    WriteByteMPU(PWR_MGMT_1, 0x00);      //解除休眠状态
    WriteByteMPU(SMPLRT_DIV, 0x02);      //333HZ采样率
    WriteByteMPU(CONFIG, 0x03);          //设置低通滤波，频率见手册    1K/ 1+2 = 333HZ
    WriteByteMPU(GYRO_CONFIG, 0x18);     //角速度计2000°/s量程
    WriteByteMPU(ACCEL_CONFIG, 0x10);    //加速度计8g量程&/
    uint8_t temp_str1[2] = {0};*/
    uint8_t temp_str1[2] = {0,0};
    WriteBufferMPU( temp_str1,USER_CTRL, 2);
    uint8_t temp_str2[4] = {0x02, 0x03, 0x18, 0x10};
    WriteBufferMPU( temp_str2, SMPLRT_DIV,4);
    printf("init mpu6050\n");
}
uint8_t MPU6050ReadID(void)
{
    unsigned char Re = 0;
    // Re=MPU_Read_Byte(MPU_DEVICE_ID_REG);
    Re = ReadByteMPU(MPU_DEVICE_ID_REG);
    // BufferReadIIC(0x75,&Re,1);
    if (Re != 0x68)
    {
        printf("地址为 %x\r\n", Re);
        return 0;
    }
    else
    {
        printf("地址为 %x\r\n", Re);
        // MPU_INFO("MPU6050 ID = %d\r\n",Re);
        return 1;
    }
}
//得到温度值
//返回值:温度值(扩大了100倍)
short MPU_Get_Temperature(void)
{
    uint8_t buf[2];
    short raw;
    float temp;
    MPU_Read_Len(MPU_ADDR, MPU_TEMP_OUTH_REG, 2, buf);
    raw = ((uint16_t)buf[0] << 8) | buf[1];
    temp = 36.53 + ((double)raw) / 340;
    return temp;
}
//得到陀螺仪值(原始值)
// gx,gy,gz:陀螺仪x,y,z轴的原始读数(带符号)
//返回值:0,成功
//    其他,错误代码
uint8_t MPU_Get_Gyroscope(short *gx, short *gy, short *gz)
{
    uint8_t buf[6], res;
    res = MPU_Read_Len(MPU_ADDR, MPU_GYRO_XOUTH_REG, 6, buf);
    if (res == 0)
    {
        *gx = ((uint16_t)buf[0] << 8) | buf[1];
        *gy = ((uint16_t)buf[2] << 8) | buf[3];
        *gz = ((uint16_t)buf[4] << 8) | buf[5];
    }
    printf("GYRO1 = %d,GYRO2 = %d,GYRO3 = %d,\r\n", *gx, *gy, *gz);
    return res;
    ;
}
/**
 * @brief           IIC初始化                                      
 *  @details        完整的初始化CH58x芯片的IIC总线
 * 
 * 
 * *//*
 * 创建者:             Winyunq
 * 创建日期:            2022-09-26
 * 
 *      《初始化》
 * 修订内容:            创建函数
 * @author          Winyunq进行完善
 * @date            2022-09-26
 * @version         1.0.0
 */
void IICInit(){
    GPIOB_ModeCfg(SCL, GPIO_ModeIN_PU);
    GPIOB_ModeCfg(SDA, GPIO_ModeIN_PU); //初始化IIC引脚
    I2C_Init(I2C_Mode_I2C, 400000, I2C_DutyCycle_16_9, I2C_Ack_Enable, I2C_AckAddr_7bit, 0);
}

void MoveDeviceInit(void)
{
    IICInit();
    MPU_Init();
    while(!MPU6050ReadID());
    printf("MPU6050加载完成");
}
/**
 * @brief           向MPU6050 IIC写入数据
 *  @details        向MPU6050所处的IIC总线上挂载的设备写入数据
 *
 * @param           参数名称:【date】         数据类型:uint8_t      需要写入的数据
 * @param           参数名称:【WriteAddress】 数据类型:uint8_t      IIC总线上接受数据设备的地址
 *
 * @return          uint8_t类型           是否产生超时
 *  @retval         【返回值分段则必选】值或范围      说明
 * */
/*
 * 创建者:             Winyunq
 * 创建日期:            2022-09-26
 *
 *      《初始化》
 * 修订内容:            创建函数
 * @author          Winyunq进行完善
 * @date            2022-09-26
 * @version         1.0.0
 */
uint8_t WriteByteMPU(uint8_t data, uint8_t WriteAddress)
{
    while(I2C_GetFlagStatus(I2C_FLAG_BUSY));                            //IIC主机判忙
    I2C_GenerateSTART(ENABLE);                                          //起始信号
    while(!I2C_CheckEvent(I2C_EVENT_MASTER_MODE_SELECT));               //判断BUSY, MSL and SB flags
    I2C_Send7bitAddress(0xD0, I2C_Direction_Transmitter);               //发送地址+最低位0表示为“写”
    while(!I2C_CheckEvent(I2C_EVENT_MASTER_TRANSMITTER_MODE_SELECTED)); //判断BUSY, MSL, ADDR, TXE and TRA flags
    I2C_SendData(WriteAddress);                                         //发送寄存器的地址

//ACK之后直接写入数据
    while(!I2C_GetFlagStatus(I2C_FLAG_TXE));                            //获取TxE的状态    数据寄存器为空标志位,可以向其中写数据
    I2C_SendData(data);                                                 //发送寄存器的地址
    while(!I2C_CheckEvent(I2C_EVENT_MASTER_BYTE_TRANSMITTED));          //判断TRA, BUSY, MSL, TXE and BTF flags
    I2C_GenerateSTOP(ENABLE);                                           //停止信号
    return 0;
}
/**
 * @brief           向MPU6050 IIC写入数据
 *  @details        向MPU6050所处的IIC总线上挂载的设备写入连续数据
 *
 * @param           参数名称:【buffer】       数据类型:uint8_t*     需要写入的数据缓冲区
 * @param           参数名称:【WriteAddress】 数据类型:uint8_t      IIC总线上接受数据设备的地址
 * @param           参数名称:【size】         数据类型:uint16_t     缓冲区大小
 *
 * @return          uint8_t类型           是否产生超时
 *  @retval         【返回值分段则必选】值或范围      说明
 * */
/*
 * 创建者:             Winyunq
 * 创建日期:            2022-09-26
 *
 *      《初始化》
 * 修订内容:            创建函数
 * @author          Winyunq进行完善
 * @date            2022-09-26
 * @version         1.0.0
 */
uint8_t WriteBufferMPU(uint8_t* buffer, uint8_t WriteAddress,uint16_t size)
{ 
    while(I2C_GetFlagStatus(I2C_FLAG_BUSY));                            //IIC主机判忙
    I2C_GenerateSTART(ENABLE);                                          //起始信号
    while(!I2C_CheckEvent(I2C_EVENT_MASTER_MODE_SELECT));               //判断BUSY, MSL and SB flags
    I2C_Send7bitAddress(0xD0, I2C_Direction_Transmitter);               //发送地址+最低位0表示为“写”
    while(!I2C_CheckEvent(I2C_EVENT_MASTER_TRANSMITTER_MODE_SELECTED)); //判断BUSY, MSL, ADDR, TXE and TRA flags
    I2C_SendData(WriteAddress);                                              //发送寄存器的地址
    for(uint8_t i=0; i<size; i++)
    {
        while(!I2C_GetFlagStatus(I2C_FLAG_TXE));                            //获取TxE的状态    数据寄存器为空标志位,可以向其中写数据
        I2C_SendData(buffer[i]);                                             //发送寄存器的地址
    }
    while(!I2C_CheckEvent(I2C_EVENT_MASTER_BYTE_TRANSMITTED));          //判断TRA, BUSY, MSL, TXE and BTF flags
    I2C_GenerateSTOP(ENABLE);                                           //停止信号
}
/**
 * @brief           用缓冲区读取MPU上的IIC的一个字节数据                                      
 *  @details        读入IIC总线上MPU6050的一个字节数据
 * 
 * @param           参数名称:【address】      数据类型:uint8_t      接收IIC总线数据的来源地址
 * 
 * @return          uint8_t类型           返回值介绍                                                         
 *  @retval         【返回值分段则必选】值或范围      说明
 * *//*
 * 创建者:             Winyunq
 * 创建日期:            2022-09-26
 * 
 *      《初始化》
 * 修订内容:            创建函数
 * @author          Winyunq进行完善
 * @date            2022-09-26
 * @version         1.0.0
 */
uint8_t ReadByteMPU(uint8_t address)
{ 
    uint8_t data = 0;
    while(I2C_GetFlagStatus(I2C_FLAG_BUSY));                            //等待IIC主机空闲
    I2C_GenerateSTART(ENABLE);                                          //起始信号
    while(!I2C_CheckEvent(I2C_EVENT_MASTER_MODE_SELECT));               //判断BUSY, MSL and SB flags
    I2C_Send7bitAddress(0xD0, I2C_Direction_Transmitter);               //发送地址+最低位0表示为“写”
    while(!I2C_CheckEvent(I2C_EVENT_MASTER_TRANSMITTER_MODE_SELECTED)); //判断BUSY, MSL, ADDR, TXE and TRA flags
    I2C_SendData(address);                                                  //发送寄存器的地址
    while(!I2C_CheckEvent(I2C_EVENT_MASTER_BYTE_TRANSMITTED));          //判断TRA, BUSY, MSL, TXE and BTF flags
    I2C_GenerateSTART(ENABLE);                                          //重起始信号，直接产生一个重起始信号即可开始读的过程
    while(!I2C_CheckEvent(I2C_EVENT_MASTER_MODE_SELECT));               //判断BUSY, MSL and SB flags
    I2C_Send7bitAddress(0xD0, I2C_Direction_Receiver);                  //发送地址+最低位1表示为“读”
    while(!I2C_CheckEvent(I2C_EVENT_MASTER_RECEIVER_MODE_SELECTED));    //判断BUSY, MSL and ADDR flags
    I2C_GenerateSTOP(DISABLE);                                          //关闭停止信号使能
    I2C_AcknowledgeConfig(DISABLE);                                     //关闭ACK使能，接收一个字节数据后，主机就回NACK表示不再接收数据
    while(!I2C_GetFlagStatus(I2C_FLAG_RXNE));                           //获取RxEN的状态，等待收到数据
    data = I2C_ReceiveData();                                           //获得从机的寄存器中的数据
    I2C_GenerateSTOP(ENABLE);                                           //停止信号
    I2C_AcknowledgeConfig(ENABLE);                                      //传输完毕，再次打开ACK使能
    return data;
}
/**
 * @brief           用缓冲区读取MPU上的IIC数据                                      
 *  @details        读入IIC总线上MPU6050的数据组
 * 
 * @param           参数名称:【Buffer】       数据类型:uint8_t*     用于接收数据的缓冲区
 * @param           参数名称:【ReadAddress】  数据类型:uint8_t      接收IIC总线数据的来源地址
 * @param           参数名称:【Length】       数据类型:uint16_t     接收数据包的长度
 * 
 * @return          uint8_t类型           返回值介绍                                                         
 *  @retval         【返回值分段则必选】值或范围      说明
 * *//*
 * 创建者:             Winyunq
 * 创建日期:            2022-09-26
 * 
 *      《初始化》
 * 修订内容:            创建函数
 * @author          Winyunq进行完善
 * @date            2022-09-26
 * @version         1.0.0
 */
uint8_t ReadBufferMPU(uint8_t* Buffer, uint8_t ReadAddress, uint16_t Length)
{
    while(I2C_GetFlagStatus(I2C_FLAG_BUSY));                            //IIC主机判忙
    I2C_GenerateSTART(ENABLE);                                          //起始信号
    while(!I2C_CheckEvent(I2C_EVENT_MASTER_MODE_SELECT));               //判断BUSY, MSL and SB flags
    I2C_Send7bitAddress(0xD0, I2C_Direction_Transmitter);               //发送地址+最低位0表示为“写”
    while(!I2C_CheckEvent(I2C_EVENT_MASTER_TRANSMITTER_MODE_SELECTED)); //判断BUSY, MSL, ADDR, TXE and TRA flags
    I2C_SendData(ReadAddress);                                                  //发送寄存器的地址
    while(!I2C_CheckEvent(I2C_EVENT_MASTER_BYTE_TRANSMITTED));          //判断TRA, BUSY, MSL, TXE and BTF flags
    I2C_GenerateSTART(ENABLE);                                          //重起始信号
    while(!I2C_CheckEvent(I2C_EVENT_MASTER_MODE_SELECT));               //判断BUSY, MSL and SB flags
    I2C_Send7bitAddress(0xD0, I2C_Direction_Receiver);                  //发送地址+最低位1表示为“读”
    while(!I2C_CheckEvent(I2C_EVENT_MASTER_RECEIVER_MODE_SELECTED));    //判断BUSY, MSL and ADDR flags
    I2C_GenerateSTOP(DISABLE);                                          //关闭停止信号使能
    for(uint8_t i=0; i<Length; i++)
    {
        if(i == Length-1)//主设备为了能在收到最后一个字节后产生一个NACK脉冲，必须在读取倒数第二个字节之后（倒数第二个RxNE 事件之后）清除ACK位(ACK=0)
        I2C_AcknowledgeConfig(DISABLE);         //清除ACK位               主设备为了能在收到最后一个字节后产生一个NACK脉冲，
        while(!I2C_GetFlagStatus(I2C_FLAG_RXNE));                       //获取RxEN的状态，等待收到数据
        Buffer[i] = I2C_ReceiveData();                                   //获得从机的寄存器中的数据
    }
    I2C_GenerateSTOP(ENABLE);                                           //使能停止信号
    I2C_AcknowledgeConfig(ENABLE);                                      //传输完毕，再次打开ACK使能
}

typedef struct{
    int64_t x;
    int64_t y;
    int64_t z;
} Vector3;
void ReadAccelerate(Vector3 *buffer)
{
    uint8_t buf[6];
    union {uint8_t buffer[2];int16_t data;}data;
    ReadBufferMPU(buf,0x3B,6);
    data.buffer[0]=buf[1];data.buffer[1]=buf[0];
    buffer->x = data.data;
    data.buffer[0]=buf[3];data.buffer[1]=buf[2];
    buffer->y = data.data;
    data.buffer[0]=buf[5];data.buffer[1]=buf[4];
    buffer->z = data.data;
}

void ReadAngle(Vector3* buffer)
{
    uint8_t buf[6];//读取缓冲区
    union {uint8_t buffer[2];int16_t data;}data;//将缓冲区的数据转化为带符号int的中间变量
    ReadBufferMPU(buf,0x43,6);//读取角速度
    data.buffer[0]=buf[1];data.buffer[1]=buf[0];//合并X数据，注意大小端，大小序问题
    buffer->x = data.data;//将合并的数据提交给X
    data.buffer[0]=buf[3];data.buffer[1]=buf[2];//合并Y数据，注意大小端，大小序问题
    buffer->y = data.data;//将合并的数据提交给Y
    data.buffer[0]=buf[5];data.buffer[1]=buf[4];//合并Z数据，注意大小端，大小序问题
    buffer->z = data.data;//将合并的数据提交给Z
}


/**
 * @brief           RISCV读取MPU上的加速度,角速度数据                                      
 *  @details        RISCV架构存在大小端，大小序问题，因此在读取MPU6050的加速度，角速度，需要调换高低位。为了增强代码的执行速度，定制ReadMPU函数，指定读入6字节数据且调换高低位
 * 
 * @param           参数名称:【Buffer】       数据类型:uint8_t*     用于接收数据的Vector函数
 * @param           参数名称:【ReadAddress】  数据类型:uint8_t      接收IIC总线数据的来源地址
 * 
 * @return          uint8_t类型           返回值介绍                                                         
 *  @retval         【返回值分段则必选】值或范围      说明
 * *//*
 * 创建者:             Winyunq
 * 创建日期:            2022-09-26
 * 
 *      《初始化》
 * 修订内容:            创建函数
 * @author          Winyunq进行完善
 * @date            2022-09-26
 * @version         1.0.0
 */
uint8_t RISCVReadMPU(uint8_t* Buffer, uint8_t ReadAddress)
{
    while(I2C_GetFlagStatus(I2C_FLAG_BUSY));                            //IIC主机判忙
    I2C_GenerateSTART(ENABLE);                                          //起始信号
    while(!I2C_CheckEvent(I2C_EVENT_MASTER_MODE_SELECT));               //判断BUSY, MSL and SB flags
    I2C_Send7bitAddress(0xD0, I2C_Direction_Transmitter);               //发送地址+最低位0表示为“写”
    while(!I2C_CheckEvent(I2C_EVENT_MASTER_TRANSMITTER_MODE_SELECTED)); //判断BUSY, MSL, ADDR, TXE and TRA flags
    I2C_SendData(ReadAddress);                                                  //发送寄存器的地址
    while(!I2C_CheckEvent(I2C_EVENT_MASTER_BYTE_TRANSMITTED));          //判断TRA, BUSY, MSL, TXE and BTF flags
    I2C_GenerateSTART(ENABLE);                                          //重起始信号
    while(!I2C_CheckEvent(I2C_EVENT_MASTER_MODE_SELECT));               //判断BUSY, MSL and SB flags
    I2C_Send7bitAddress(0xD0, I2C_Direction_Receiver);                  //发送地址+最低位1表示为“读”
    while(!I2C_CheckEvent(I2C_EVENT_MASTER_RECEIVER_MODE_SELECTED));    //判断BUSY, MSL and ADDR flags
    I2C_GenerateSTOP(DISABLE);                                          //关闭停止信号使能，
    while(!I2C_GetFlagStatus(I2C_FLAG_RXNE));                       //获取RxEN的状态，等待收到数据
    Buffer[1] = I2C_ReceiveData(); 
    while(!I2C_GetFlagStatus(I2C_FLAG_RXNE));                       //获取RxEN的状态，等待收到数据
    Buffer[0] = I2C_ReceiveData(); 
    while(!I2C_GetFlagStatus(I2C_FLAG_RXNE));                       //获取RxEN的状态，等待收到数据
    Buffer[3] = I2C_ReceiveData(); 
    while(!I2C_GetFlagStatus(I2C_FLAG_RXNE));                       //获取RxEN的状态，等待收到数据
    Buffer[2] = I2C_ReceiveData(); 
    while(!I2C_GetFlagStatus(I2C_FLAG_RXNE));                       //获取RxEN的状态，等待收到数据
    Buffer[5] = I2C_ReceiveData(); 
    I2C_AcknowledgeConfig(DISABLE);         //清除ACK位               主设备为了能在收到最后一个字节后产生一个NACK脉冲，
    while(!I2C_GetFlagStatus(I2C_FLAG_RXNE));                       //获取RxEN的状态，等待收到数据
    Buffer[4] = I2C_ReceiveData(); 
    I2C_GenerateSTOP(ENABLE);                                           //使能停止信号
    I2C_AcknowledgeConfig(ENABLE);                                      //传输完毕，再次打开ACK使能
}